#include "chassis_task.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "chassis.h"
#include "can_motor_user.h"
#include "locate_task.h"
#include "uart.h"

float x;
float y;
float z;
float p;


void StartChassisTask (void *argument);
osThreadId_t	chassisTaskHandle ;		//电机调速任务（速度环pid调速）
osThreadAttr_t  chassisTask_attributes;

void Angle_ReviseTask(void *argument);
osThreadId_t	AngleTaskHandle ;		//角度修正任务
osThreadAttr_t  AngleTask_attributes ;



void chassis_task_init(void)
{
    {
        chassisTask_attributes.name = "chassisTask";
        chassisTask_attributes.stack_size = 128 * 4;
        chassisTask_attributes.priority = (osPriority_t) osPriorityNormal;		//优先级normal
    }
    {
        AngleTask_attributes.name = "AngleTask";
        AngleTask_attributes.stack_size = 128 * 4;
        AngleTask_attributes.priority = (osPriority_t) osPriorityNormal;		//优先级normal
    }
    //创建任务
    chassisTaskHandle = osThreadNew (StartChassisTask , NULL, &chassisTask_attributes  );
    //AngleTaskHandle   = osThreadNew (Angle_ReviseTask , NULL, &AngleTask_attributes  );
}

int start = 0;//暂停键
extern uint8_t error;
extern uint8_t bluetooth_flag;
extern uint8_t xbox_A;
//extern uint8_t xbox_Up;       //目前被发射占用
//extern uint8_t xbox_Left;     //目前被发射占用
extern float Z;
float revise_z;


/**
 * @brief 底盘输出任务
 */
void StartChassisTask ( void *argument)
{
    for(;;)
    {
        //串口断联检测
        if(error)
        {
            all_stop();
        }

        //蓝牙断联检测
        if(bluetooth_flag)//蓝牙状态正常
        {
            //所有电机强制停转检测
            if(xbox_A==0)
            {
                //平动修正
                if(z == 0 && (x != 0 || y != 0))  //平动时才进行pid修正
                {

                    if(y <= 0)
                    {
                        getValues(-x, y, z);
                    }
                    else
                    {
                        getValues(-x, y, z + y / 9.0f);
                    }

                    //if(x<=0)getValues(-x,y,x/7.0f);
                    //else getValues(-x,y,y/8.0f);
                    //getValues(-x,y,z);//跟随手柄移动
                }

                //非平动不进行修正
                else
                {
                    getValues(-x,y,z);//跟随手柄移动
                }
            }

            else if(xbox_A==1)
            {
                all_stop();	//所有的电机停止输出
            }

            //目前不用自瞄
#if 0
            if(xbox_Up==1)  //自瞄
            {
                getValues(0,0,Z);
            }
#endif
        }

        else
        {
            all_stop();
            getValues(0,0,0);

        }

        osDelay(1);
    }
}

/**
 * @brief 底盘陀螺仪修正任务
 */
void Angle_ReviseTask ( void *argument)
{

    //revise_z = -Angular_velocity_cal()/1000.0f;

}


